#include "VESC.h"
#include <stdlib.h>

static VESC_Status **gpVESC_Status = NULL;
static uint8_t gMotorNum = 0;

static void VESC_Error_Handler(void);

enum VESC_Control_Mode
{
    VESCDutyMode = 0,
    VESCCurrentLoopMode,
    VESCBreakMode,
    VESCSpeedLoopMode,
    VESCPosationMode,
    VESCGetERPM_Current_DutyCycle = 9,
    VESCGetTempFet_TempMotor_CurrentIn_Position = 16,
};


void VESC_currentMode(VESC_HandleTypeDef* hvesc, float Current);
void VESC_speedMode(VESC_HandleTypeDef* hvesc, float Speed);
void VESC_PosMode(VESC_HandleTypeDef* hvesc, float Pos);
void VESC_staticBreakMode(VESC_HandleTypeDef* hvesc, float BreakCurrent);
void VESC_dynamicBreakMode(VESC_HandleTypeDef* hvesc, float BreakCurrent);
void VESC_dutyMode(VESC_HandleTypeDef* hvesc, float Duty);

uint8_t VESC_getCurrent(VESC_HandleTypeDef* hvesc, float *current);
uint8_t VESC_getSpeed(VESC_HandleTypeDef* hvesc, float *speed);
uint8_t VESC_getPos(VESC_HandleTypeDef* hvesc, float *pos);

static struct VESC_pf vesc_pf = 
{
    .dynamicBreakMode = VESC_dynamicBreakMode,
	.staticBreakMode = VESC_staticBreakMode,
	.currentMode = VESC_currentMode,
    .speedMode = VESC_speedMode,
    .dutyMode = VESC_dutyMode,
    .posMode = VESC_PosMode,
	.getCurrent = VESC_getCurrent,
	.getPos = VESC_getPos,
	.getSpeed = VESC_getSpeed,
};


void VESC_Init(VESC_HandleTypeDef *hvesc, UserCAN_HandleTypeDef *hcan, uint8_t ID, uint8_t Poles, uint8_t gear_ratio)
{
	
#ifdef __CAN_H__
    if (hcan->Instance == CAN1)
    {
        USER_CAN1_INIT(hcan);
    }
#ifdef CAN2
    else if (hcan->Instance == CAN2)
    {
        USER_CAN2_INIT(hcan);
    }
#endif
	
#elif defined __FDCAN_H__
	if (hcan->Instance == FDCAN1)
    {
        USER_FDCAN1_INIT(hcan);
    }
    else if (hcan->Instance == FDCAN2)
    {
        USER_FDCAN2_INIT(hcan);
    }
    else if (hcan->Instance == FDCAN3)
    {
        USER_FDCAN3_INIT(hcan);
    }
#endif
    else VESC_Error_Handler();
	
	hvesc->status.ID = ID;
    hvesc->status.hcan = hcan;
	hvesc->status.Poles = Poles;
	hvesc->status.gear_ratio = gear_ratio;
	hvesc->pf = &vesc_pf;
}



#ifdef __CAN_H__

void VESC_CAN_Transmit(uint8_t ID, UserCAN_HandleTypeDef *hcan, enum VESC_Control_Mode Mode, const uint8_t *Data, uint8_t len)
{
    if(len > 8) VESC_Error_Handler();
    if (hcan == NULL) VESC_Error_Handler();

    CAN_TxHeaderTypeDef Can_Tx;
    uint32_t pTxMailbox = 0;
    Can_Tx.StdId = 0;
    Can_Tx.ExtId = ID | (Mode << 8);
    Can_Tx.IDE = CAN_ID_EXT;
    Can_Tx.RTR = 0;
    Can_Tx.DLC = len;

	while(HAL_CAN_GetTxMailboxesFreeLevel(hcan )==0){}

    HAL_CAN_AddTxMessage(hcan, &Can_Tx, Data, &pTxMailbox);
}


#elif defined __FDCAN_H__

	void VESC_CAN_Transmit(uint8_t ID, UserCAN_HandleTypeDef *hcan, enum VESC_Control_Mode Mode, const uint8_t *Data, uint8_t len)
	{
		if(len > 8) VESC_Error_Handler();
		if (hcan == NULL) VESC_Error_Handler();

		FDCAN_TxHeaderTypeDef CAN_Tx;

		CAN_Tx.Identifier = ID | (Mode << 8);
		CAN_Tx.IdType = FDCAN_EXTENDED_ID;
		CAN_Tx.TxFrameType = FDCAN_DATA_FRAME;
		CAN_Tx.DataLength = len;
		CAN_Tx.ErrorStateIndicator = FDCAN_ESI_ACTIVE;
		CAN_Tx.BitRateSwitch = FDCAN_BRS_OFF;
		CAN_Tx.FDFormat = FDCAN_CLASSIC_CAN;
		CAN_Tx.TxEventFifoControl = FDCAN_NO_TX_EVENTS;
		CAN_Tx.MessageMarker = 0;

		while(HAL_FDCAN_GetTxFifoFreeLevel(hcan)==0){}

		HAL_FDCAN_AddMessageToTxFifoQ(hcan, &CAN_Tx, Data);
	}

#endif

	
void VESC_RxInit(uint8_t VESCNum, VESC_HandleTypeDef **VESCArray)
{
	static bool VESC_RxInitFlag = 0;
	if((VESCArray != NULL)&&(VESC_RxInitFlag==0))
	{
		VESC_RxInitFlag = 1;
		gMotorNum = VESCNum;
		gpVESC_Status = (VESC_Status **)malloc(VESCNum * sizeof(*gpVESC_Status));
		for(uint8_t i=0;i<VESCNum;i++)
		{
			gpVESC_Status[i] = &VESCArray[i]->status;
		}
	}
}


void VESC_currentMode(VESC_HandleTypeDef* hvesc, float Current)
{
    int32_t Current_buff1 = Current * 1000;
    uint8_t Current_buff2[4];

    Current_buff2[0] = Current_buff1 >> 24;
    Current_buff2[1] = Current_buff1 >> 16;
    Current_buff2[2] = Current_buff1 >> 8;
    Current_buff2[3] = Current_buff1;

    VESC_CAN_Transmit(hvesc->status.ID, hvesc->status.hcan, VESCCurrentLoopMode, Current_buff2, 4);
}



void VESC_speedMode(VESC_HandleTypeDef* hvesc, float Speed)
{
    Speed = Speed * hvesc->status.Poles;

    int32_t Speed_buff = Speed;

    uint8_t Speed_buff1[4];

    Speed_buff1[0] = Speed_buff >> 24;
    Speed_buff1[1] = Speed_buff >> 16;
    Speed_buff1[2] = Speed_buff >> 8;
    Speed_buff1[3] = Speed_buff;

    VESC_CAN_Transmit(hvesc->status.ID, hvesc->status.hcan, VESCSpeedLoopMode, Speed_buff1, 4);
}


void VESC_PosMode(VESC_HandleTypeDef* hvesc, float Pos)
{
    Pos /= hvesc->status.gear_ratio;
    Pos = Pos * 1000000;

    int32_t Pos_buff = Pos;

    uint8_t Pos_buff1[4];

    Pos_buff1[0] = Pos_buff >> 24;
    Pos_buff1[1] = Pos_buff >> 16;
    Pos_buff1[2] = Pos_buff >> 8;
    Pos_buff1[3] = Pos_buff;

    VESC_CAN_Transmit(hvesc->status.ID, hvesc->status.hcan, VESCPosationMode, Pos_buff1, 4);
}


void VESC_staticBreakMode(VESC_HandleTypeDef* hvesc, float BreakCurrent)
{
    int32_t Current_buff1 = BreakCurrent * 1000;
    uint8_t Current_buff2[4];

    Current_buff2[0] = Current_buff1 >> 24;
    Current_buff2[1] = Current_buff1 >> 16;
    Current_buff2[2] = Current_buff1 >> 8;
    Current_buff2[3] = Current_buff1;

    VESC_CAN_Transmit(hvesc->status.ID, hvesc->status.hcan, 12, Current_buff2, 4);
}

void VESC_dynamicBreakMode(VESC_HandleTypeDef* hvesc, float BreakCurrent)
{
    int32_t Current_buff1 = BreakCurrent * 1000;
    uint8_t Current_buff2[4];

    Current_buff2[0] = Current_buff1 >> 24;
    Current_buff2[1] = Current_buff1 >> 16;
    Current_buff2[2] = Current_buff1 >> 8;
    Current_buff2[3] = Current_buff1;

    VESC_CAN_Transmit(hvesc->status.ID, hvesc->status.hcan, 2, Current_buff2, 4);
}

void VESC_dutyMode(VESC_HandleTypeDef* hvesc, float Duty)
{
    Duty = Duty * 1000;

    int32_t Duty_buff = Duty;
    uint8_t Duty_buff1[4];

    Duty_buff1[0] = Duty_buff >> 24;
    Duty_buff1[1] = Duty_buff >> 16;
    Duty_buff1[2] = Duty_buff >> 8;
    Duty_buff1[3] = Duty_buff;

    VESC_CAN_Transmit(hvesc->status.ID, hvesc->status.hcan, VESCDutyMode, Duty_buff1, 4);
}

#if defined __FDCAN_H__

uint8_t VESC_Analysis(user_FDCAN_Rx_Struct *FDCAN_Rx_struct)
{
	union u16_i16 sU16_i16 = {0};
	union u32_i32 sU32_i32 = {0};
	if (FDCAN_Rx_struct->FDCAN_RxHeader.IdType != FDCAN_EXTENDED_ID)
	{
		return 0;
	}
		
	
	
	if (FDCAN_Rx_struct->FDCAN_RxHeader.DataLength != 8)
	{
		return 0;
	}
	
	if (((FDCAN_Rx_struct->FDCAN_RxHeader.Identifier>>8)&0xff)==9)
	{
		uint8_t ID = (FDCAN_Rx_struct->FDCAN_RxHeader.Identifier&0xff);
		
		for(uint8_t i=0;i<gMotorNum;i++)
		{
			if (ID == gpVESC_Status[i]->ID)
			{
				sU16_i16.u16 = (FDCAN_Rx_struct->Data[4] << 8) | FDCAN_Rx_struct->Data[5];
				sU32_i32.u32 = (FDCAN_Rx_struct->Data[0] << 24) | (FDCAN_Rx_struct->Data[1] << 16) | (FDCAN_Rx_struct->Data[2] << 8) | FDCAN_Rx_struct->Data[3];
				
				gpVESC_Status[i]->current = (float)(sU16_i16.i16 / 10.f);
				gpVESC_Status[i]->speed = (float)(sU32_i32.i32 / gpVESC_Status[i]->Poles);	//除以极对数ERPM换算成RPM
				return ID;
			}
		}
	} else if (((FDCAN_Rx_struct->FDCAN_RxHeader.Identifier>>8)&0xff)==16)
	{
		uint8_t ID = (FDCAN_Rx_struct->FDCAN_RxHeader.Identifier&0xff);
		
		for(uint8_t i=0;i<gMotorNum;i++)
		{
			if (ID == gpVESC_Status[i]->ID)
			{
				sU16_i16.u16 = (FDCAN_Rx_struct->Data[6] << 8) | FDCAN_Rx_struct->Data[7];
				gpVESC_Status[i]->posation = (float)(sU16_i16.i16 / 50.f);
				gpVESC_Status[i]->posRxFlag = 1;
				return ID;
			}
		}
	}
	
	
	
	return 0;
}

uint8_t VESC_getCurrent(VESC_HandleTypeDef* hvesc, float *current)
{
	if (hvesc->status.current == 0)
	{
		return 0;
	}
	*current = hvesc->status.current;
	return 1;
}

uint8_t VESC_getSpeed(VESC_HandleTypeDef* hvesc, float *speed)
{
	if (hvesc->status.speed == 0)
	{
		return 0;
	}
	*speed = hvesc->status.speed;
	return 1;
}

uint8_t VESC_getPos(VESC_HandleTypeDef* hvesc, float *posation)
{
	if ((hvesc->status.posation == 0)||(hvesc->status.posRxFlag == 0))
	{
		return 0;
	}
	*posation = hvesc->status.posation;
	hvesc->status.posRxFlag = 0;
	return 1;
}



#endif


void VESC_Error_Handler()
{
    __disable_irq();
    while (1);
}

